//
// Created by fl on 23-9-5.
//

#ifndef SIGMA_GUIDANCE_PX4_INTERFACE_H
#define SIGMA_GUIDANCE_PX4_INTERFACE_H

#include "mavlink/v2.0/standard/mavlink.h"
#include <arpa/inet.h>
#include <mutex>
#include "get_time.h"
#include <utility>
#include <thread>
#include <unistd.h>
#include <queue>
#include <atomic>
#include "matrix/math.hpp"
#include "mathlib/mathlib.h"

struct UavStatus {
    bool valid{};                      // 编队算法使用位置数据时进行判断
    uint64_t timestamp{};              // 收到数据的时间，用于判断本数据是否有效
    mavlink_global_position_int_t globalPos{};
    mavlink_attitude_quaternion_t att_q{};
    mavlink_home_position_t home_pos{};
    mavlink_vfr_hud_t vfr_hud{};
    mavlink_heartbeat_t heartbeat{};
};

enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
    PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};

class Px4Interface {
public:
    Px4Interface(int recv_port = 26540, int send_port = 24580, 
                const std::string& px4_ip = "127.0.0.1");
    ~Px4Interface();

    //获取自身飞控状态量
    UavStatus getPx4Status();

    bool updated(UavStatus &px4_info);

    // 发送外部控制指令到飞控，并切换飞行模式到offboard
    void send_att_cmd_to_px4(mavlink_set_attitude_target_t &cmd) const;

    // 发送欧拉角控制量到飞控，单位弧度
    void send_euler_cmd_to_px4(float roll, float pitch, float yaw, 
                                float throttle);

    // 发送北东地坐标，单位米
    void send_local_pos_target_to_px4(float n, float e, float d);

    // 发送mission指令
    void set_mission_mode() const;

    void terminate_offboard() const;

    // 发送offboard指令
    void set_offboard_mode() const;

    // 发送解锁指令
    void send_arm_to_px4() const;

    // 针对固定翼，发送takeoff指令，北东地坐标，单位米
    void send_takeoff_to_px4(float n, float e, float d); 

private:
    bool initUDP(int recv_port, int send_port, const std::string& px4_ip);

    void recvPX4byUDP();

    // 对UDP缓存区的内容进行解包
    void decodeUDPmsg();

    void sendHeartbeat();

    bool send_msg_to_px4(const mavlink_message_t &msg) const;

private:
    uint8_t  sys_id_{1};

    std::atomic<bool> updated_{false};

    int sock_fd_{};
    struct sockaddr_in  st_recv_addr_{};
    struct sockaddr_in  st_send_addr_{};

    UavStatus  px4_info_;
    std::mutex px4_info_mtx_;

    std::queue<char> udpdata_;
    std::mutex udpdata_mtx_;

    mavlink_heartbeat_t my_heartbeat_;
    uint8_t main_mode_;
};

#endif //SIGMA_GUIDANCE_PX4_INTERFACE_H
